/*=========================================================================
	UberSim Source Code Release
	-------------------------------------------------------------------------
	Copyright (C) 2002 Manuela Veloso, Brett Browning, Mike Bowling,
	James Bruce; {mmv, brettb, mhb, jbruce}@cs.cmu.edu
	Erick Tryzelaar {erickt}@andrew.cmu.edu
	School of Computer Science, Carnegie Mellon University
	-------------------------------------------------------------------------
	This software is distributed under the GNU General Public License,
	version 2.  If you do not have a copy of this licence, visit
	www.gnu.org, or write: Free Software Foundation, 59 Temple Place,
	Suite 330 Boston, MA 02111-1307 USA.  This program is distributed
	in the hope that it will be useful, but WITHOUT ANY WARRANTY,
	including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
	-------------------------------------------------------------------------*/

#include <stdio.h>
#include <ode/ode.h>
#include <typeinfo>

#include "mmgr.h"

#include "Collision.h"
#include "CollisionController.h"
#include "CollisionGroup.h"
#include "CollisionManager.h"
#include "Engine.h"
#include "Node.h"
#include "Geometry.h"
#include "TriangleMesh.h"
#include "RigidBody.h"
#include "RigidBodyCollider.h"
#include "Simulator.h"
#include "Surface.h"

/*******************************/

CollisionManager* CollisionManager::getInstance ()
{
	static CollisionManager instance;

	return &instance;
}

/*******************************/

CollisionManager::~CollisionManager ()
{
	shutdown ();
}

/*******************************/

void CollisionManager::initialize ()
{
	if (!isInitialized)
	{
		isInitialized = true;

		isShutdown = false;
	}
}

/*******************************/

void CollisionManager::shutdown ()
{
	if (!isShutdown)
	{
		isShutdown = true;

		for (unsigned int i = 0; i < collisionGroups.size (); i++)
		{
			CollisionGroup* collisionGroup = collisionGroups[i];

			if (collisionGroup)
			{
				delete collisionGroup;
			}
		}

		isInitialized = false;
	}
}

/*******************************/

int CollisionManager::addCollisionGroup (CollisionGroup* collisionGroup)
{
	for (unsigned int i = 0; i < collisionGroups.size (); i++)
	{
		if (collisionGroups[i] == 0)
		{
			collisionGroups[i] = collisionGroup;

			return i;
		}
	}

	collisionGroups.push_back (collisionGroup);
	return collisionGroups.size ();
}

/*******************************/

CollisionGroup* CollisionManager::removeCollisionGroup (unsigned int i)
{
	if (0 < collisionGroups.size () && 0 <= i && i < collisionGroups.size ())
	{
		CollisionGroup* collisionGroup = collisionGroups[i];
		collisionGroups[i]             = 0;

		return collisionGroup;
	}

	return 0;
}

/*******************************/

void CollisionManager::removeCollisionGroup (CollisionGroup* collisionGroup)
{
	for (unsigned int i = 0; i < collisionGroups.size (); i++)
	{
		if (collisionGroups[i] == collisionGroup)
		{
			collisionGroups[i] = 0;
		}
	}
}

/*******************************/

void CollisionManager::beginFrame ()
{
}

/*******************************/

void CollisionManager::frame ()
{
	for (unsigned int i =0; i < collisionGroups.size (); i++)
	{
		if (collisionGroups[i])
		{
			collisionGroups[i]->testForCollisions (this);
		}
	}

}

/*******************************/

void CollisionManager::endFrame ()
{
}

/*******************************/

void CollisionManager::addCollision (Geometry* geometry1, Spatial* spatial2)
{
	Node* node2 = dynamic_cast<Node *> (spatial2);

	if (node2)
	{
		spatial2->onCollidingWith (geometry1);
		return;
	}

	Geometry* geometry2 = dynamic_cast<Geometry *> (spatial2);

	if (geometry2)
	{
		addCollision (geometry1, geometry2);
	}
}

/*******************************/

void CollisionManager::addCollision (Geometry* geometry1, Geometry* geometry2) 
{
	//printf ("geometry1: %s geometry2: %s\n", geometry1->getName (), geometry2->getName ()); 

	dContact contact[3];

	int numberOfCollisions = dCollide (geometry1->getGeometry (), geometry2->getGeometry (), 
			3, &contact[0].geom, sizeof (dContact));

	if (0 < numberOfCollisions)
	{
		//printf ("collided -> geometry1: %s geometry2: %s\n", geometry1->getName (), geometry2->getName ()); 

		CollisionController* collisionController1 = geometry1->getCollisionController ();
		CollisionController* collisionController2 = geometry2->getCollisionController ();

		if (collisionController1 || collisionController2)
		{
			for (int i = 0; i < numberOfCollisions; i++)
			{	
				Vector3 position = contact[i].geom.pos;
				Vector3 normal   = contact[i].geom.normal;
				Real    depth    = contact[i].geom.depth;

				Collision collision = Collision (position, normal, depth);
				collision.contact = &contact[i];

				if (collisionController1)
				{
					collisionController1->handleCollision (geometry2, collision);
				}

				if (collisionController2)
				{
					collisionController2->handleCollision (geometry1, collision);
				}
			}
		}
	}
}

/*******************************/

CollisionManager::CollisionManager ()
{
	isInitialized = false;
	isShutdown    = true;
}

/*******************************/
